#include "matrix.h"
#include <cstring>
#include <cmath>

namespace webrtc {
Matrix::Matrix() {
  Reset();
}

void Matrix::Reset() {
  values = {
      1, 0, 0,
      0, 1, 0,
      0, 0, 1
  };
}

void Matrix::Set(int index, float value) {
  values[index] = value;
}

float Matrix::Get(int index) {
  return values[index];
}

void Matrix::GetValues(std::array<float, 9> &outValues) const {
  outValues = values;
}

void Matrix::SetValues(const std::array<float, 9>& inValues) {
  values = inValues;
}

void Matrix::PreTranslate(float dx, float dy) {
  PreConcat(Matrix::CreateTranslate(dx, dy));
}

void Matrix::PreScale(float sx, float sy) {
  PreConcat(Matrix::CreateScale(sx, sy));
}

void Matrix::PreScale(float sx, float sy, float px, float py) {
  Matrix scaleMatrix = CreateScale(sx, sy);
  Matrix translateMatrix = CreateTranslate(px * (1 - sx), py * (1 - sy));

  PreConcat(translateMatrix);
  PreConcat(scaleMatrix);
}

void Matrix::PreRotate(float degrees) {
  PreConcat(Matrix::CreateRotate(degrees));
}

Matrix Matrix::CreateScale(float sx, float sy) {
  Matrix matrix;
  matrix.Set(Matrix::MSCALE_X, sx);
  matrix.Set(Matrix::MSCALE_Y, sy);
  return matrix;
}

Matrix Matrix::CreateRotate(float degrees) {
  Matrix matrix;
  float radians = degrees * M_PI / 180;
  float cosValue = cos(radians);
  float sinValue = sin(radians);
  matrix.Set(Matrix::MSCALE_X, cosValue);
  matrix.Set(Matrix::MSKEW_X, -sinValue);
  matrix.Set(Matrix::MSKEW_Y, sinValue);
  matrix.Set(Matrix::MSCALE_Y, cosValue);
  return matrix;
}

Matrix Matrix::CreateTranslate(float dx, float dy) {
  Matrix matrix;
  matrix.Set(MTRANS_X, dx);
  matrix.Set(MTRANS_Y, dy);
  return matrix;
}

void Matrix::PreConcat(const Matrix& other) {
  values = Matrix::Multiply(values, other.values);
}

void Matrix::PostTranslate(float dx, float dy) {
  PostConcat(Matrix::CreateTranslate(dx, dy));
}

void Matrix::PostScale(float sx, float sy) {
  PostConcat(Matrix::CreateScale(sx, sy));
}

void Matrix::PostScale(float sx, float sy, float px, float py) {
   PostTranslate(-px, -py);
   PostScale(sx, sy);
   PostTranslate(px, py);
}

void Matrix::PostRotate(float degrees) {
  PostConcat(Matrix::CreateRotate(degrees));
}

void Matrix::PostConcat(const Matrix& other) {
  values = Matrix::Multiply(other.values, values);
}

std::array<float, 9> Matrix::Multiply(const std::array<float, 9>& lhs, const std::array<float, 9>& rhs) {
  std::array<float, 9> result{};
  result[Matrix::MSCALE_X] = lhs[Matrix::MSCALE_X] * rhs[Matrix::MSCALE_X] + lhs[Matrix::MSKEW_X] * rhs[Matrix::MSKEW_Y] + lhs[Matrix::MTRANS_X] * rhs[Matrix::MPERSP_0];
  result[Matrix::MSKEW_X] = lhs[Matrix::MSCALE_X] * rhs[Matrix::MSKEW_X] + lhs[Matrix::MSKEW_X] * rhs[Matrix::MSCALE_Y] + lhs[Matrix::MTRANS_X] * rhs[Matrix::MPERSP_1];
  result[Matrix::MTRANS_X] = lhs[Matrix::MSCALE_X] * rhs[Matrix::MTRANS_X] + lhs[Matrix::MSKEW_X] * rhs[Matrix::MTRANS_Y] + lhs[Matrix::MTRANS_X] * rhs[Matrix::MPERSP_2];
  result[Matrix::MSKEW_Y] = lhs[Matrix::MSKEW_Y] * rhs[Matrix::MSCALE_X] + lhs[Matrix::MSCALE_Y] * rhs[Matrix::MSKEW_Y] + lhs[Matrix::MTRANS_Y] * rhs[Matrix::MPERSP_0];
  result[Matrix::MSCALE_Y] = lhs[Matrix::MSKEW_Y] * rhs[Matrix::MSKEW_X] + lhs[Matrix::MSCALE_Y] * rhs[Matrix::MSCALE_Y] + lhs[Matrix::MTRANS_Y] * rhs[Matrix::MPERSP_1];
  result[Matrix::MTRANS_Y] = lhs[Matrix::MSKEW_Y] * rhs[Matrix::MTRANS_X] + lhs[Matrix::MSCALE_Y] * rhs[Matrix::MTRANS_Y] + lhs[Matrix::MTRANS_Y] * rhs[Matrix::MPERSP_2];
  result[Matrix::MPERSP_0] = lhs[Matrix::MPERSP_0] * rhs[Matrix::MSCALE_X] + lhs[Matrix::MPERSP_1] * rhs[Matrix::MSKEW_Y] + lhs[Matrix::MPERSP_2] * rhs[Matrix::MPERSP_0];
  result[Matrix::MPERSP_1] = lhs[Matrix::MPERSP_0] * rhs[Matrix::MSKEW_X] + lhs[Matrix::MPERSP_1] * rhs[Matrix::MSCALE_Y] + lhs[Matrix::MPERSP_2] * rhs[Matrix::MPERSP_1];
  result[Matrix::MPERSP_2] = lhs[Matrix::MPERSP_0] * rhs[Matrix::MTRANS_X] + lhs[Matrix::MPERSP_1] * rhs[Matrix::MTRANS_Y] + lhs[Matrix::MPERSP_2] * rhs[Matrix::MPERSP_2];
  return result;
}

void Matrix::MapPoints(float dst[], const float src[], int point_count) const {
  for (int i = 0; i < point_count; i++) {
    int index = i * 2;
    float x = src[index];
    float y = src[index + 1];

    dst[index] = x * values[MSCALE_X] + y * values[MSKEW_X] + values[MTRANS_X];
    dst[index + 1] = x * values[MSKEW_Y] + y * values[MSCALE_Y] + values[MTRANS_Y];
  }
}

void Matrix::MapRect(RectF &dst, const RectF &src) const {
  float points[8];
  points[0] = src.left;
  points[1] = src.top;
  points[2] = src.right;
  points[3] = src.top;
  points[4] = src.left;
  points[5] = src.bottom;
  points[6] = src.right;
  points[7] = src.bottom;

  MapPoints(points, points, 4);

  float min_x = points[0];
  float max_x = points[0];
  float min_y = points[1];
  float max_y = points[1];

  for(int i = 2; i < 8; i += 2) {
    if(min_x > points[i]) min_x = points[i];
    if(max_x < points[i]) max_x = points[i];
    if(min_y > points[i+1]) min_y = points[i+1];
    if(max_y < points[i+1]) max_y = points[i+1];
  }

  dst.left = min_x;
  dst.top = min_y;
  dst.right = max_x;
  dst.bottom = max_y;
}

bool Matrix::IsIdentity() const {
  if (values[MSCALE_X] != 1.0f || values[MSCALE_Y] != 1.0f || values[MPERSP_2] != 1.0f)
    return false;

  if (values[MSKEW_X] != 0.0f || values[MTRANS_X] != 0.0f ||
      values[MSKEW_Y] != 0.0f || values[MTRANS_Y] != 0.0f ||
      values[MPERSP_0] != 0.0f || values[MPERSP_1] != 0.0f)
    return false;

  return true;
}

void Matrix::inverseMatrixPoint(float dst[], const float src[]) {
  Matrix inserse;
  if (Matrix::Invert(inserse)) {
    inserse.MapPoints(dst, src, 1);
  }
}

bool Matrix::Invert(Matrix& inverse) const {
  float det = values[0] * (values[4] * values[8] - values[7] * values[5]) - values[1] * (values[3] * values[8] - values[5] * values[6]) +
              values[2] * (values[3] * values[7] - values[4] * values[6]);

  if (det == 0.0f) {
    return false;
  }

  float invDet = 1.0f / det;
  std::array<float, 9> inv = {
      invDet * (values[4] * values[8] - values[5] * values[7]),  invDet * -(values[1] * values[8] - values[2] * values[7]),
      invDet * (values[1] * values[5] - values[2] * values[4]),

      invDet * -(values[3] * values[8] - values[5] * values[6]), invDet * (values[0] * values[8] - values[2] * values[6]),
      invDet * -(values[0] * values[5] - values[3] * values[2]),

      invDet * (values[3] * values[7] - values[4] * values[6]),  invDet * -(values[0] * values[7] - values[1] * values[6]),
      invDet * (values[0] * values[4] - values[1] * values[3])};

  inverse.SetValues(inv);
  return true;
}
}